#include "auto_focus.h"
#include "image_process.h"
#include <QDebug>
#include <QThread>
#include <fstream>

using namespace cv;

extern ImageProcess* imageProcess;

AutoFocus::AutoFocus(QObject* parent)
	: QObject(parent),
	focusEvaluation(nullptr),
	focusSearch(nullptr),
	searchRange(3),
	isCaptured(false),
	isBorder(false)
{
	focusEvaluation = new FourierEvaluation;
	focusSearch = new TraverseSearch;

	connect(&timer, &QTimer::timeout, [&]() {
		mutex.lock();
		isCaptured = true;
		mutex.unlock();
	});
}

AutoFocus::~AutoFocus()
{
	delete focusEvaluation;
	delete focusSearch;
}

void AutoFocus::start(int msec)
{
	timer.setInterval(msec);
	timer.start();
}

void AutoFocus::stop()
{
	timer.stop();
}

void AutoFocus::process()
{
	if (imageProcess->empty(this)) {
		return;
	}

	Mat image = imageProcess->read(this);
	if (image.empty()) {
		return;
	}

	double focusValue = focusEvaluation->evaluate(image);
	focusValues.enqueue(focusValue);
	
	if (focusValues.length() > searchRange) {
		focusValues.dequeue();
	}

	focusSearch->setBorder(isBorder);
	isBorder = false;
	focusSearch->setFocusPos(lensCtrl.getPos());
	FocusAction focusAction = focusSearch->search(focusValues);

	qDebug() << lensCtrl.getPos() << " " << focusValues << "direction = " << focusAction.action << "\tstep = " << focusAction.value;

	isBorder = lensCtrl.adjust(focusAction);
}

bool AutoFocus::connectSerialPort(const QString& serialName)
{
	bool ok = lensCtrl.connectSerialPort(serialName);
	focusSearch->setMaxFocus(lensCtrl.getMaxFocusPos());
	focusSearch->setFocusPos(lensCtrl.getPos());

	return ok;
}

